Gradient-based autonomous obstacle avoidance trajectory planning for B-spline UAVs

Unmanned aerial vehicles (UAVs) have become the focus of current research because of their practicability in various scenarios. However, current local path planning methods often result in trajectories with numerous sharp or inflection points, which are not ideal for smooth UAV flight. This paper introduces a UAV path planning approach based on distance gradients. The key improvements include generating collision-free paths using collision information from initial trajectories and obstacles. Then, collision-free paths are subsequently optimized using distance gradient information. Additionally, a trajectory time adjustment method is proposed to ensure the feasibility and safety of the trajectory while prioritizing smoothness. The Limited-memory BFGS algorithm is employed to efficiently solve optimal local paths, with the ability to quickly restart the trajectory optimization program. The effectiveness of the proposed method is validated in the Robot Operating System simulation environment, demonstrating its ability to meet trajectory planning requirements for UAVs in complex unknown environments with high dynamics. Moreover, it surpasses traditional UAV trajectory planning methods in terms of solution speed, trajectory length, and data volume.


Trajectory initialization
The UAV trajectory should be collision-free, with the shortest path length, and conform to the smooth curve of UAV dynamics.The B-spline is capable of generating a curve that precisely satisfies these conditions in the given environment.The expression of the B-spline is shown in Eq. (1).
where C i represents the control point, B i,k represents the B-spline basis function of order K , and u represents the node vector.To simplify B-spline generation and improve the efficiency of path generation, the optimal 4th-order basis functions are used to generate B-spline curves.The curve segmentation generates each B-spline trajectory by four control points and four B-spline basis functions calculated by Eq. (2) where s from 0 to 1 represents the normalized distance, and each segment of the trajectory is generated as shown in Eq. (3).
where C 1 , C 2 , C 3 , C 4 is the four control points of the trajectory and P i belongs to R 3 .The complete trajectory of the UAV is stitched by each segment of the trajectory, and the next segment is generated by the control points C 2 , C 3 , C 4 , C 5 .When there are N control points, N-3 segments of UAV trajectories are generated.The velocity V (s) and acceleration A(s) of the position curve can be deduced from 3. (1) (−3s 3 + 3s 2 + 3s + 1) Since the control points of the trajectory are immobile, differentiating the trajectory is differentiating the spline function.Then the velocity spline functions V 1 , V 2 , V 3 , V 4 , and the acceleration spline functions a 1 , a 2 , a 3 , a 4 can be obtained.Therefore, both the velocity and acceleration of the B-spline trajectory P i are functions of the parameter S, as shown in Eqs.(8) and (9).This shows that the end point of the former curve and the start point of the latter curve are equal in position, velocity, and acceleration, which ensures that the B-spline trajectory is smooth and continuous.It is also assumed that additional control points are added to verify that the trajectory passes through the start point and end point.We expand the starting point C 1 as c 0 , c 1 , and c 2 , where c 1 is the origin C 1 , c 0 and c 2 as in Eq. (10).
where v 1 represents the velocity vector when the UAV passes through the point, L is a suitable constant, and for the experiment we take half the distance required for the UAV to gain maximum velocity.Solving Eq. ( 9) by substituting it into Eqs.(7) and (8) verifies that the trajectory passes through the starting point C 1 and has velocity v 1 acceleration a 1 of zero.Similarly, the target point C n is extended to c n−1 , c n ,c n+1 and c n+1 , where c n is the origin C n , c n−1 and c n+1 are denoted by Eq. (11).
The solution of the equation also verifies that the trajectory passes through the control point C N , the speed of the UAV is v n , and the acceleration a n is zero.Therefore as long as we adjust the control points C 1 , C 2 , C 3 , C 4 , …, C N , the initialization of the UAV B-spline trajectory can be completed after solving.
In order to quickly initialize the UAV B-spline trajectory, the polynomial fitting method is used for the selection of control points.After obtaining the starting point C 1 and the ending point C N of the trajectory, the maximum singular value d z between the two points in space is calculated and compared with the set distance d c of the distribution of the control points, to determine whether to add a waypoint.If d z is larger, it is necessary to set waypoints.The expression for the number m of setting waypoints is: The coordinates of the path point C N are calculated as: www.nature.com/scientificreports/After obtaining each path point it is known that m-1 segments of UAV trajectories are generated, and the polynomial coefficients of each UAV trajectory in the x-axis, y-axis, and z-axis directions, F x , F y , F z are solved as shown in Eq. (14).
where, B x , B y , B z are the parameter vectors in the x-axis, y-axis, and z-axis directions at the beginning and end of each UAV trajectory, and their expressions are: where x 0 , y 0 , z 0 , v x0 , v y0 , v z0 , a x0 , a y0 , a z0 are the coordinate, velocity, and acceleration of each segment of the trajectory on the three axes of the starting point.x n , y n , z n , v xn , v yn , v zn , a xn , a yn , a zn are the coordinate, veloc- ity, and acceleration of the corresponding segment of the trajectory on the three axes of the ending point.The parameter matrix in Eq. ( 14) is expressed as: where t represents the time required to pass through the corresponding trajectory at the UAV's maximum speed, and the time is set to be twice the time required to fly at the maximum speed, considering that the UAV may not be able to maintain the maximum speed during the start and end segments.t n can be formulated as follows.
After obtaining the coordinates of the waypoints and the polynomial coefficients of the m-1 group of coordinates, the polynomial fitting is used to generate the coordinates of the control points, and the solution formula is shown in Eq. ( 18). ( 14) where C is the coefficient matrix associated with the number of waypoints and K x , K y , K z are the augmentation vector of the coefficient vector F x , F y , F z of the polynomial.After obtaining the control points according to Eq.
The trajectory initialization process has only the starting point and the endpoint of the trajectory, and to facilitate the trajectory solving, the initialization process of the trajectory is calculated using the parameter S, but the trajectory optimization process is more convenient to use the time parameter t to solve the trajectory.The conversion formula for the parameters S and t of the UAV trajectory is:

Optimization of trajectory Clamped B-splines
According to de Boor-Cor's recursive formula (21), it can be shown that each higher-order B-spline is a convex linear combination of two lower first-order B-splines.Each segment of the B-spline curve must have enough basis functions to match the control points if the interval is legal.Therefore, in the proposed algorithm, n + 4 node vectors (u 0 , • • • , u n+3 ) are required if there are n control points.Four nodes (u i , u i+1 , u i+2 , u i+3 ) are needed if one wants to determine a 4th-order B-spline P i .Neighboring B-splines have 2 control points that are the same to ensure that the splicing between every two segments is good.
The UAV trajectory initialization process, by assuming the increase of control points to ensure that the UAV passes through the trajectory start point, the solution passes through is the beginning and end of the two points of the velocity of v 1 and v n , respectively.However, the speed of the UAV in the beginning two points can only be zero.Therefore, in the trajectory solution process outlined in this paper, the first four and the last four node vectors remain consistent.This B-spline is called clamped B-spline and its structure is shown in Fig. 1.
The graph features 10 control points and 14 nodes with node vectors [0, 0, 0, 0, 0, 0.125, 0.25, 0.375, 0.5, 0.625, 0.75, 0.875, 1, 1, 1, 1] T .Figure 1 perfectly illustrates the two main properties of the clamped B-spline.The curve passes through the head and tail control points and has a strong convex hull 25,26 .Strong convex hull property means that if the node vectors u are located in [u i , u i+1 ] , then the curve P(u) is located within the convex hull defined by control points C i , C i−1 , C i−2 , C i−3 (i ≥ 3) , which indirectly verifies that a control point only affects local trajectories.The B-spline function is localized, and this property can solve the problem of local changes in the trajectory affecting the overall trajectory planning when the UAV encounters an obstacle.The collision part of the trajectory is changed by adjusting the relevant control points, while the convex hull of the B-spline ensures that the trajectory meets the restricted range.

Obstacle avoidance optimization
When a trajectory collides with an obstacle, the control point of the collision is selected, and the corresponding point of the control point is generated on the surface of the obstacle.Each control point C i may have more than one corresponding point ρ i,j on the obstacle surface, but each corresponding point ρ i,j belongs to only one control point.According to the corresponding repulsive direction vector H i,j generated by the positions and ρ i,j , the relationship between the three is shown in Fig. 2. i coincide with the index of the original control point, and j is the index of the corresponding point and the opposite direction vector.The distance between the obstacle surface point and the original control point is calculated as shown in Eq. (22).
Since the trajectory is continuously collision detected and optimized, to avoid the resulting duplicates of H i,j and ρ i,j , we consider obstacles encountered by the control points C i as newly discovered obstacles only by restricting D i,j > 0. This restriction helps us to find obstacles that affect the new trajectory, reducing the amount of computation that can be done to update the new trajectory faster.
Environment as an important reference factor in UAV obstacle avoidance algorithms, a large amount of environmental data must be stored as a prerequisite for obstacle avoidance, and at the same time, appropriate trajectory algorithms are established to avoid obstacles.This means that there is a large amount of data to be computationally processed, which is undoubtedly an insurmountable problem for the arithmetic power of micro UAVs.The article algorithm only needs to store the environmental data in the narrow space near the B-spline trajectory, while the repulsive force algorithm is simple and efficient, which greatly reduces the amount of computation 27 .Moreover, because it does not require a lot of environmental information, it avoids the problem of UAVs falling into local minima and does not require collision-free trajectories to be generated in advance.
Since the trajectory is generated based on the B-spline curve, it is affected by the selection of basis function and control point, the B-spline curve structure is shown in Fig. 3.The distance between control points has a great influence on trajectory generation.If the distance between the trajectory control points is too large, the trajectory deformation caused by each iteration of the trajectory will be large, and unnecessary UAV performance waste will occur when facing small obstacles.If the distance between control points is too small, more iterative calculations are needed and even failed to planning when facing large obstacles.

Gradient-based trajectory optimization
According to Eq. ( 12) it can be seen that in the B-spline parameterization process, we use a uniform B-spline so that the time interval t in the control points is the same and very small, so the UAV velocity v i , acceleration a i , and jerk j i between the two control points can be expressed as: Vol.:(0123456789)The trajectory planning task for UAVs is carried out in a differentially flat space with set control points C i , C i ∈ R 3 , so the trajectory optimization process transforms into: where J s is the smoothness penalty, J c represents the collision penalty, J f represents the feasibility penalty, s , c and f are the weights of the three penalties.
Smoothness penalty: the smoothness penalty is formulated as the curvature of the trajectory, the smoothness of the trajectory is judged by the geometric information of the human-computer trajectory, if the higher curvature of the trajectory represents more difficulty for the UAV to track the trajectory.Therefore, the functional expression of the smoothing penalty is: Minimizing each control point acceleration a i and jerk j i , makes the whole trajectory smooth.Collision penalties: the purpose of establishing collision penalties is to keep the UAV away from obstacles and keep the two at a safe distance s f .A segmentation function is established to determine the rate of change of the collision penalty based on the size of D i,j .Therefore, the expression of the collision penalty function for the corresponding point ρ i,j of each control point is: where j c (i, j) is the cost of generating the repulsive direction vector H i,j at the corresponding point ρ i,j on the control point C i .The cost of each control point is calculated independently, and when a control generates multiple repulsive direction vectors, it represents an increase in the cost of obstacle avoidance.Thus, the collision penalty incurred by a control point is calculated as: where e is the number of control points generating repulsive direction vectors H .The cost of a UAV trajectory collision is the accumulation of the collision penalty J c (i) for all control points.Therefore, the total cost of the collision penalty is: To obtain the total cost of collision J c , we obtain the gradient by deriving it with the expression: Feasibility penalty: we need to constrain the UAV trajectory in x, y, and z directions to ensure that the speed, acceleration, and jerk of the UAV in each dimension satisfy the UAV performance constraints.Due to the convex envelopment of the B-spline, we can restrict the UAV trajectory by restricting the derivatives of the control points.
where w v , w a and w j are the weights of the feasibility penalty in terms of velocity, acceleration and jerk.F is a quadratically continuous differentiable function of the higher-order derivatives of the control points.( 23) where c r ∈ C ∈ v i , a i , j i , a 1 , b 1 , c 1 , a 2 , b 2 and c 2 are able to satisfy the second-order continuity of the function F .c m is the limit of the derivatives, c j is the node of the second and third intervals.is the elasticity coefficient and 0 < < 1 to make the final result satisfy the constraints.

Time adjustment
The time T i of an individual trajectory segment P i is mainly determined by the number and position of control points.In the initialization phase of the trajectory, we assign times to individual segments.When the UAV changes its path due to an obstacle, the speed V i , acceleration A i and jerk J i of the UAV exceed the UAV's own limits, needs to adjust the time, the expression for the time conversion ratio is: where v m , a m and j m respectively represent maximum speed, acceleration, and jerk on the x, y, and z-axis.The time for the UAV to complete the reassignment of the path fragments is as follows:

Numerical solution
This paper proposes the use of gradient for trajectory optimization, which can be seen as solving extreme value problems of multivariate objective functions 28 .The optimal approach for solving such unconstrained optimization problems is to use the Newton method.By utilizing the solved data during the objective function generation process, the requirement for the fast restart of trajectory computation can be met, and repeated computation can be avoided, thereby improving the solving speed.However, solving the UAV trajectory requires extremely high real-time performance.In the solving process of Newton's method, the inverse of the Hessian matrix needs to be computed, which exceeds the computational power of the UAV.Therefore, the proposed quasi-Newton algorithm is used for solving 29-31 .
There are various quasi-Newton algorithms, and in this paper, the L-BFGS algorithm is utilized 32-34 .This algorithm ensures a high success rate in obstacle avoidance and also exhibits good performance in terms of solving accuracy and restart loss.The solution process is as follows, x ∈ R 3 , f (x) and the update formula for x is: where λ is the learning rate and the iterative formula for H K is: where H K also does not need to be solved explicitly, and H K is approximated by B k .Thus, the proposed Newtonian condition is:  The iterative formula for the algorithmic matrix B k+1 can be obtained after obtaining the appropriate P k and Q k : The L-BFGS algorithm requires H 0 k to be a positive definite matrix in order to ensure gradient descent.To achieve convergence, a monotone line search is performed under strong Wolfe conditions, with the Hessian matrix H 0 k being used.

Experiment results and analysis
Utilizing the ROS platform 35 , we conducted UAV autonomous flight experiments across various simulated environments and compared them with other algorithms to assess the proposed algorithm's effectiveness.During the experiment, the UAV faced limitations in acquiring global obstacle information.When approaching an obstacle, the UAV was able to gather local obstacle information through a simulated camera.This information may not even be all the information about an obstacle.Initially, the simulation experiment was conducted in a setting with simple obstacles.In experiments, the UAV flew from an initial point to a target point at a constant speed and stable attitude, while traversing an area with four obstacles.Four obstacles were distributed within the simulation map of [25.0 m, 25.0 m, 5.0 m], consisting of three cylinders with varying thicknesses and a wall.Experiments can intuitively reflect the trajectory generation process of UAVs.When the trajectory generated in the previous iteration fails to avoid obstacles, the proposed algorithm can quickly generate a new path.The experimental environment is depicted in Fig. 4.
The UAV trajectory is generated in segments, without considering collision during the initialization process.The B-spline order is 3, resulting in approximately 25 control points per segment of the trajectory.Each segment has a horizontal length of around 7 m, with an initial distance interval of about 0.3 m between neighboring points.The safety distance between the UAV and obstacles is set at 0.8 m.These parameters are considered optimal as they effectively balance the degrees of freedom and complexity of the UAV trajectory.Experiment 1 involves generating the UAV running trajectory using the B-spline algorithm in a simple obstacle environment.The total length of the trajectory is 30.956 m.The process of UAV running obstacle avoidance is illustrated in Fig. 5, and the UAV's three-axis speed is shown in Fig. 6.In this paper, the proposed path planning algorithm is compared with two traditional graph search algorithms, A* and Jump Point Search (JPS), as well as the Bezier curve fitting algorithm.Three algorithms require sensing most of the obstacles in the environment in order to discover the optimized path.The article algorithm focuses more on path optimization compared to the other three algorithms, particularly in a simple environment, resulting in smaller data processing.The paths generated by the three algorithms in the identical experimental environment are compared, and the results are shown in Fig. 7.
Based on the findings presented in Fig. 7 and Table 1, it can be observed that the A* and JPS algorithms demonstrate better performance in terms of time efficiency in simple environments.This is mainly because the obstacle environment is relatively simple, and the data processing amount is too small.When faced with more complex environments, the advantages of algorithms will be shown.Meanwhile, these algorithms are not as effective as the algorithm proposed in the paper in terms of the trajectory length and the number of nodes used.On the other hand, the B-spline trajectory planning method algorithm and Bezier curve algorithm generate smoother trajectories that are well-suited for UAV dynamics and can be easily implemented in real-life scenarios.
To further validate the effectiveness of the algorithm proposed in this paper, Experiment 2 randomly generated 300 cylindrical obstacles and Circular obstacles within a square range of 50 m on each side.The position, radius, and height of each cylindrical obstacle were randomly generated, and there was no overlap between the obstacles.Three path planning methods operate independently in the same environment mentioned above.The UAV initiates its flight from the coordinates [− 25, − 25, 5] and navigates towards the final destination point 5,  25 .The resulting trajectories generated by the UAV using the three trajectory calculation methods are illustrated in Fig. 8.
The results presented in Fig. 8 and Table 2 indicate that the proposed algorithm effectively tackles real-time obstacle avoidance in randomized and complex environments.While the A*, JPS, and Bezier curve algorithms also demonstrate success in solving the obstacle avoidance problem during the experiment, they struggle to handle the significant amount of data generated by more complex environments, leading to suboptimal solutions.This highlights the superior stability of the algorithms proposed in this paper.The trajectory shape in Fig. 8 demonstrates that the proposed algorithm in the paper primarily achieves obstacle avoidance through in-plane roundabouts.This approach ensures the stability of the UAV while increasing the solving speed and generating a smoother UAV trajectory.
In addition, we conducted the autonomous flight obstacle avoidance simulation experiment of UAVs in L, H, U, and omega under four special obstacle environments, and the generated UAV trajectories are shown in Fig. 9.In these cases, the algorithm still provides a safe and smooth UAV trajectory that allows the UAV to traverse these obstacles.
When the UAV passes L-shaped obstacles, a smooth trajectory is generated, as shown in Fig. 9a.The UAV initially fits the control points by polynomial and connects the control points to generate the trajectory.When no obstacle is found, the trajectory flies along the line between the starting point and the destination place, as shown in Fig. 9b,c, which may cause the UAV to waste kinetic energy.Of course, this is limited by the UAV's ability to sense obstacles, and similar situations can be avoided by carrying better sensors.Secondly, when the algorithm pursued smoothness, the UAV circled a part of the distance, as shown in Fig. 9c,d.However, the UAV is easier to complete the smoother trajectories than the sharp ones without requiring global obstacle information, and it has higher adaptability to the unknown environment.
Under the same conditions of obstacle generation methods, boundary constraints, and route constraints, the four algorithms operate independently in environments with ten different numbers of obstacles.The comparison of trajectory lengths generated by the four algorithms is presented in Fig. 10.In 10 obstacle density environments, the average trajectory lengths for the four algorithms are 71.98 m, 72.50 m, 73.69 m, and 73.47 m respectively.The comparison in Fig. 10 reveals that the proposed algorithm exhibits a shorter trajectory and runtime.

Conclusion
This paper focuses on developing a B-spline curve-based UAV path planner to achieve a smoother and more suitable flight path.The proposed algorithm demonstrates comparable performance in planning distance and time to other advanced path planning algorithms while producing a trajectory that is better suited for UAV flight.In addition, the algorithm only requires obstacle information in the narrow space around the trajectory, leading to reduced data processing and enabling the UAV to navigate through unknown obstacle environments swiftly and safely.Simulation experiments confirmed the effectiveness and reliability of the algorithm.

Figure 3 .
Figure 3. Change of B-spline curve by control points selection.

Table 1 .
Path comparison of the three algorithms.
AlgorithmsLengths (m) Time (s) Nodes used Figure 8.Comparison of trajectories in Experiment 2.

Table 2 .
Comparison of paths in complex environments.